function [sys, x0]=sfuntf_mod(t,x,u,flag,...
                          fftpts,npts,HowOften,offset,ts,averaging,...
                          freq_of_interest,makeplot,dev_test_plots)
% sfuntf_mod() is a modified version of Mathworks' sfuntf() Simulink
% s-function.  The modifcation is to accept one more input and generate
% two more outputs.

% The new inputs are : (1) the desired frequency at which the amplitude
%                          and phase are to be output (freq_of_interest in rad/s)
%                      (2) a flag to indicate the users choice for
%                          graphical output or not (makeplot==0->no plotting,
%                          makeplot==1->plotting)
% The new outputs are: the amplitude and phase angle at the desired
%                      output frequency
%                      - amplitude is expressed in units equal to the output's
%                        units divided by the input's units (which, or course,
%                        is problem specific)
%                      - phase angle is in degrees
%
% The general idea of the unmodified sfuntf() S-function is to capture
% some (ideally oscillatory) input and output signals, take the fft of
% both of them, and output the numerically derived real-time amplitude ratio
% and phase angle difference of the output and input.
% - For strictly linear time-invariant systems at steady state, this represents
%   a numerically generated bode plot over the frequency range afforded by the
%   fft's sampling window size.  The results are presented for all frequencies
%   afforded by the fft's frequency window and, assuming the window is sufficiently
%   large to caputure several waveforms, is likely valid for (most of) the
%   frequency range shown.
% - For nonlinear systems, the amplitude and phase angle represents a numerically
%   derived instantaneous describing function and likely is not valid for the entire
%   frequency spectrum generated by the fft.
%   My assumption is that the amplitude and phase angle are, in fact, close to
%   correct for the input, or fundamental, frequency.
%   Assuming you can control the input frequency passed into the nonlinear
%   system, this is the frequency of interest at which the amplitude and phase
%   should be pretty close to correct.
%
% Marc Compere
% CompereM@saic.com
% first modified: 13 February 2003
% last modified : 13 February 2003

%SFUNTF an S-function which performs transfer function analysis using ffts.
%   This M-file is designed to be used in a Simulink S-function block.
%   It stores up a buffer of input and output points of the system
%   then plots the frequency response of the system based on this information.
%
%   The input arguments are:
%     npts:        number of points to use in the fft (e.g. 128)
%     HowOften:    how often to plot the ffts (e.g. 64)
%     offset:      sample time offset (usually zeros)
%     ts:          how often to sample points (secs)
%     averaging:   whether to average the transfer function or not
%
%   The spectrum analyzer displays three plots: the time history,
%   the phase and magnitude of the transfer function.

%   Copyright 1990-2002 The MathWorks, Inc.
%   $Revision: 1.24 $
%   Andrew Grace 5-30-91.
%   Revised Wes Wang 4-28-93, 6-18-96.

if abs(flag) == 2, % Flag equals 2 on a real time hit (mdlUpdate)
  % Is it a sample hit ?
  nstates = (2*npts + 2 + averaging*(2*round(fftpts/2)) + 2 + 2)/2;
  sys = zeros(nstates,2);
  sys(:) = x; % the (:) takes the 1D x vector and reshapes it into the 2D sys array
  h_fig = sys(nstates, 2);
  if (h_fig <= 0 & makeplot==1),
    % Graph initialization

    sl_name = gcs;
    blocks = get_param(sl_name, 'CurrentBlock');
    [n_b, m_b]= size(blocks);
    if n_b < 1
      error('Cannot delete block while simulating')
    elseif n_b > 1
      error('Something wrong in get_param, You don''t have the current Simulink')
    end
    % findout if the graphics window exist
    ind = find(sl_name == setstr(10));
    for i = ind
      sl_name(i) = '_';
    end
    Figures = get(0,'Chil');
    new_figure = 1;
    for i = 1:length(Figures)
      if strcmp(get(Figures(i), 'Type'), 'figure')
        if strcmp(get(Figures(i), 'Name'), sl_name)
          h_fig = Figures(i);
          handles = get(h_fig,'UserData');
          if length(handles) == 9
            new_figure = 0;
            %handles = [h_sub311, h_plot1, h_plot2, h_sub312, h_plot1, h_plot2, h_sub313, h_plot1, h_plot2]
            for j=1:3
              set(handles(j*3-2),'Visible','off');
              set(handles(j*3-1),'XData',0,'YData',0,'Erasemode','none');
              set(handles(j*3),  'XData',0,'YData',0,'Erasemode','none');
              xl = get(handles(j*3-2),'Xlabel');
              set(xl, 'String','');
              tl = get(handles(j*3-2),'Title');
              set(tl, 'String','');
            end
            yl = get(handles(7), 'Ylabel');
            set(yl,'String','')
          end
        end
      end
    end
    if new_figure
      h_fig = figure('Unit','pixel','Pos',[100 100 400 500],'Name',sl_name);
      set(0, 'CurrentF', h_fig);
      %subplot311
      handles(1) = subplot(311);
      handles(2) = plot(0,0,'m','EraseMode','None');
      set(handles(1),'NextPlot','add');
      handles(3) = plot(0,0,'c','EraseMode','None');
      set(handles(1),'Visible','off');
      set(handles(1),'NextPlot','new');
      %subplot312
      handles(4) = subplot(312);
      handles(5) = plot(0,0,'EraseMode','None');
      handles(6) = handles(5);
      set(handles(4),'Visible','off');
      %subplot313
      handles(7) = subplot(313);
      handles(8) = plot(0,0,'EraseMode','None');
      handles(9) = handles(8);
      set(handles(7),'Visible','off');
    end
    tl = get(handles(4),'Title');
    set(tl, 'String','Working - Please Wait');
    set(h_fig, 'UserData', handles);
    set(h_fig, 'NextPlot', 'new')
    sys(nstates,2) = h_fig; % place figure handle at the end of the sys array
    drawnow;
  end % if (h_fig <= 0 & makeplot==1)

  sample_time=0; % assume this timestep is not a sample time to update
                 % the magnitude and phase information
  if rem(t - offset + 1000*eps, ts) < 10000*eps, % update magnitude and phase information
    x(1,1) = x(1,1) + 1; % increment counter
    sys(x(1,1),:) = u(:)'; % insert the current input at the counter position
                           % in sys() overwriting whatever was there
    div = x(1,1)/HowOften;
    if (div == round(div))
      sample_time=1; % set the flag to indicate what information to put on the output
                     % in the output function (i.e. flag==3 or mdlOutputs())

      % generate the data sets in correct time order by picking off the
      % array extending from the counter+1'th element in sys (i.e. element
      % number x(1,1)+1 ) to npts+1 (i.e. the end of the dataset array)
      % in both the first (input) and second (output) columns.  Also
      % tack on the rest of the data that extends from the beginning of
      % the data region (i.e. sys(2,1) and sys(2,2)) to the counter, x(1,1).

      % The data set extends in both columns from 2:npts+1, namely
      % sys(2:npts+1,1) and sys(2:npts+1,2)

      ya = [sys(x(1,1)+1:npts+1,1);sys(2:x(1,1),1)]; % data set a (input)
      yb = [sys(x(1,1)+1:npts+1,2);sys(2:x(1,1),2)]; % data set b (output)
      if dev_test_plots>=2, % plot the buffer as it is accumulated
         figure(101)
         subplot(2,1,1)
            plot(ya)
            ylabel('ya')
            title('a buffer contents')
         subplot(2,1,2)
            plot(yb)
            ylabel('yb')
            title('b buffer contents')
         xlabel('buffer index')
         grid
      end

      n = fftpts/2;
      freq = 2*pi*(1/ts); % Multiply by 2*pi to get radians/s (hmm... sampling frequency is also 2x max freq discernable by fft based on N_sample points...?)
      w = freq*(0:n-1)./(2*(n-1)); % (rad/s) omega

      % Hanning window to remove transient effects at the
      % beginning and end of the time sequence.
      nw = min(fftpts, npts);
      win = .5*(1 - cos(2*pi*(1:nw)'/(nw+1)));

      ga = fft(win.*ya(1:nw),fftpts);
      gb = fft(win.*yb(1:nw),fftpts);

      if dev_test_plots>=2,
         Paa = ga .* conj(ga) / fftpts;
         figure(102)
         plot(w/(2*pi),Paa(1:length(w)))
         title('Frequency Content of Input Signal')
         ylabel('Power Spectrum')
         xlabel('Frequency (Hz)')
         grid
      end

      % Perform averaging with overlap if number of fftpts
      % is less than buffer

      ng = fftpts;

      % Averaging without overlap: (this never executes if npts=fftpts)
      while (ng <= (npts - fftpts))
              ga = (ga + fft(win.*ya(ng+1:ng+nw),fftpts)) / 2;
              gb = (gb + fft(win.*yb(ng+1:ng+nw),fftpts)) / 2;
              ng = ng + n; % For no overlap set: ng = ng + fftpts;
      end

      % protect against divide-by-zero errors (i.e. putting Nan's into the g array)
      ga_zero_indicies = find(ga(1:n)==0); % locate all entried in ga that are zero
      ga(ga_zero_indicies) = eps;

      g = gb(1:n)./ga(1:n); % g = output/intput, with real and imaginary components

      method=1;
      if method==1,
         phase = (180/pi)*unwrap(atan2(imag(g),real(g))); % (deg)
      else, % use alternative method
         phase_a = (180/pi)*unwrap(atan2(imag(ga(1:n)),real(ga(1:n)))); % phase of input in deg w.r.t. to beginning of sampling window
         phase_b = (180/pi)*unwrap(atan2(imag(gb(1:n)),real(gb(1:n)))); % phase of input in deg w.r.t. to beginning of sampling window
         phase = phase_b - phase_a; % phase difference (deg)
      end

      mag  = abs(g); % units are specific to the units of signal a and signal b

      % Averaging:
      if averaging, % use the second half of the (npts x 2) sys() array
        cnt  = sys(1,2); % retrieve averaging counter
        sys(npts+2:npts+1+n,1) = cnt/(cnt+1) *  sys(npts+2:npts+1+n,1) + mag/(cnt + 1);
        sys(npts+2:npts+1+n,2) = cnt/(cnt+1) *  sys(npts+2:npts+1+n,2) + phase/(cnt + 1);
        sys(1,2) = sys(1,2) + 1; % update the second counter
        mag = sys(npts+2:npts+1+n,1);
        phase = sys(npts+2:npts+1+n,2);
      end

      if makeplot==1,
          h_fig = sys(nstates, 2); % retrieve figure handle
          handles = get(h_fig,'UserData');
	
          if averaging
            tmp = 'Averaged Transfer Function ';
          else
            tmp = 'Transfer Function ';
          end

          % plot the raw time-series data
          tvec = (t - ts * npts + ts * (1:npts));
          set(handles(1),'Visible','on','Xlim',[min(tvec), max(tvec)],'Ylim',[min(min([ya;yb])), max(max([ya;yb+eps]))])
          set(handles(2),'XData',tvec,'YData',ya)
          set(handles(3),'XData',tvec,'YData',yb);
          tl = get(handles(1),'Title');
          set(tl, 'String','Time history (1st: magenta; 2nd: cyan)');
          xl = get(handles(1), 'Xlabel');
          set(xl, 'String', 'Time (secs)')
	
          % plot the calculated phase angle data
          ysc = phase(~isnan(phase));
          if isempty(ysc)
            ysc=[0 1];
          else
            ysc = [min(ysc), max(ysc+eps)];
          end
          set(handles(7),'Visible','on','Xlim',[min(w) max(w)],'Ylim',ysc)
          set(handles(8),'XData',w,'YData',phase)
          tl = get(handles(7),'Title');
          set(tl, 'String',[tmp '(phase)'])
          xl = get(handles(7), 'Xlabel');
          set(xl, 'String', 'Frequency (rads/sec)')
          yl = get(handles(7), 'Ylabel');
          set(yl, 'String','Degrees')
	
          % plot the calculated amplitude data
          ysc = mag(~isnan(mag));
          if isempty(ysc)
            ysc=[0 1];
          else
            ysc = [min(ysc), max(ysc+eps)];
          end
          set(handles(4),'Visible','on','Xlim',[min(w) max(w)],'Ylim',ysc)
          set(handles(5),'XData',w,'YData',mag)
          xl = get(handles(4), 'Xlabel');
          set(xl, 'String', 'Frequency (rads/sec)')
          tl = get(handles(4),'Title');
          set(tl, 'String',[ tmp '(magnitude)'])
      end % if makeplot==1

    end % if (div == round(div))

    if sys(1,1) == npts
      x(1,1) = 1; % reset counter if it points to the last entry in the discrete states
    end

    sys(1,1) = x(1,1);

  end % if rem(t - offset + 1000*eps, ts) < 10000*eps
%disp('--- begin output within Flag==2 ---')
%sample_time
%sys(nstates-1,:)
  % interpolate to pick out the magnitude and phase at the frequency of interest
  % only when w exists (indicating a sample time has just occurred)
  if sample_time==1, % new magnitude and phase information is available
     %size(w); % w --> 1x(npts/2)
     %size(mag) % mag --> (npts/2)x1 
     %size(phase) % phase --> (npts/2)x1
     if dev_test_plots>=1,
        figure(10); if dev_test_plots==3, clf, end
        if ishold==0,hold on,end
        plot(w,mag); xlabel('\Omega (rad/s)'); ylabel('Magnitude'); grid
        if ishold==1,hold off,end
        figure(11); if dev_test_plots==3, clf, end
        if ishold==0,hold on,end
        plot(w,phase); xlabel('\Omega (rad/s)'); ylabel('Phase (degrees)'); grid
        if ishold==1,hold off,end
     end % if testing
     mag_of_interest = interp1(w,mag,freq_of_interest,'linear');
     phase_of_interest = interp1(w,phase,freq_of_interest,'linear');
     sys(nstates-1,:) = [mag_of_interest phase_of_interest];
  %else, % output whatever got placed there from the last mag & phase update time
  %   sys(nstates-1,:) = [1 2];
  end

  sys = sys(:); % collapse the 2-column sys into a long single columm
%disp('--- end output within Flag==2 ---')

elseif flag == 3, % Return next sample hit (mdlOutputs)
%disp('--- begin output within Flag==3 ---')
  nstates = (2*npts + 2 + averaging*(2*round(fftpts/2)) + 2 + 2)/2;

  % cast the 1D x array into a 2D array
  sys = zeros(nstates,2);
  sys(:) = x; % the (:) takes the 1D x vector and reshapes it into the 2D sys array
%size(sys)
  % pick off the mag and phase and at the same time collapse sys down to a 1x2
  sys=sys(nstates-1,:); % sys(1) is magnitude, sys(2) is phase in degrees
                        % at the freq_of_interest
%sys=[1 2];
%disp('--- end output within Flag==3 ---')
elseif flag == 4, % Return next sample hit (mdlGetTimeOfNextVarHit)
  ns = (t - offset)/ts;  % Number of samples
  sys = offset + (1 + floor(ns + 1e-13*(1+ns)))*ts;

elseif flag  == 0, % Initialization (mdlInitializeSizes)
  % Number of discrete states for storage:
  nstates = 2*npts + 2 + averaging*(2*round(fftpts/2)) + 2 + 2;

  % Notes on state usage:
  % The nstates line above used to appear as:
  %    nstates = 2*npts + 2 + averaging*(2*round(fftpts/2)) + 2
  % Adding 2 to nstates allows for persistent storage of the (scalar)
  % amplitude and phase information at the freq_of_interest.
  % The third "+ 2" must also be added to the lines at the beginning of the
  % flag==2 and flag==3 sections.
  % - The first row (both entries) x(1,1) and x(1,2) are used as counters.
  % - The next npts x 2 block contain the streaming input of current data
  % - The next optional fftpts x 2 block contains averaging information
  % - There are "+ 2" and "+ 2" more states that I suspect are either
  %   exactly sized to be used in the indexing or are just +1 fudge factors
  %   and are unused (haven't taken the time to figure out the indexing down
  %   to the single entry)
  % - The very last entry in x (i.e. ) is used for storing figure handles
  %   (this is a clue that the second "+ 2" is being used properly and that
  %    the first "+ 2" is not necessary although I'm not certain)
  % - By adding the last "+ 2", this provides a row in which to store two scalar
  %   pieces of magnitude and phase information at x(nstates-1,:)

  %      SYS(1) = Number of continuous states.
  %      SYS(2) = Number of discrete states.
  %      SYS(3) = Number of outputs.
  %      SYS(4) = Number of inputs.
  %               Any of the first four elements in SYS can be specified
  %               as -1 indicating that they are dynamically sized. The
  %               actual length for all other flags will be equal to the
  %               length of the input, U.
  %      SYS(5) = Reserved for root finding. Must be zero.
  %      SYS(6) = Direct feedthrough flag (1=yes, 0=no). The s-function
  %               has direct feedthrough if U is used during the FLAG=3
  %               call. Setting this to 0 is akin to making a promise that
  %               U will not be used during FLAG=3. If you break the promise
  %               then unpredictable results will occur.
  %      SYS(7) = Number of sample times. This is the number of rows in TS.
  sys = [0;nstates;2;2;0;0];              % Sizes of system (see SFUNTMPL)
  x0 = zeros(nstates/2,2);                % Initial conditions
  x0(1,1) = 1;                            % Set counter to 1.
  if HowOften > npts
    error('The number of points in the buffer must be more than the plot frequency')
  end
  x0(nstates/2,2) = 0;
  x0 = x0(:);

  if dev_test_plots==1,
     figure(10)
     clf
     figure(11)
     clf
  end

else                                            % Other flag options ignored
  sys = [];
end

% sfuntf
